#include "IntegralImage.h"
#include "Point.h"
#include "Plane.h"
#include "Line.h"



float ** estimateDepthEdge(IntegralImage <float> ii_depth, IntegralImage <float> ii_valid_depth,int size,int width,int height)
{
	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}
	
	
	for(int i = size; i < width-size; i++)
	{
		for(int j = size; j < height-size; j++)
		{
			int ipsize = i+size;
			int jpsize = j+size;
			int insize = i-size;
			int jnsize = j-size;
			
			float px 			= ii_depth.sum(			ipsize,	jpsize,	i,		jnsize);
			float divpx 		= ii_valid_depth.sum(	ipsize,	jpsize,	i,		jnsize);
			if(divpx > 0)		{px /= divpx;}
			
			float nx 			= ii_depth.sum(			i,		jpsize,	insize,	jnsize);
			float divnx 		= ii_valid_depth.sum(	i,		jpsize,	insize,	jnsize);
			if(divnx > 0)		{nx /= divnx;}
			
			float dx 			= fabs(px-nx);

			
			float py 			= ii_depth.sum(			ipsize,	jpsize,	insize,	j);
			float divpy 		= ii_valid_depth.sum(	ipsize,	jpsize,	insize,	j);
			if(divpy > 0)		{py /= divpy;}
			
			float ny 			= ii_depth.sum(			ipsize,	j,		insize,	jnsize);
			float divny 		= ii_valid_depth.sum(	ipsize,	j,		insize,	jnsize);
			if(divny > 0)		{ny /= divny;}
			
			float dy 			= fabs(py-ny);
			float div 			= ii_valid_depth.sum(	ipsize,	jpsize,	insize,	jnsize);
			float total 		= ii_depth.sum(			ipsize,	jpsize,	insize,	jnsize);
			if(divpx < size || divnx < size || divpy < size || divny < size){
				edge[i][j] 		= -1;
			}else{
				edge[i][j] 		= 20.0f*sqrt(dx*dx+dy*dy)/(total/div);
			}
		}
	}
	return edge;
}

float ** estimateNormalEdge(IntegralImage <float> ii_norm_x, IntegralImage <float> ii_norm_y, IntegralImage <float> ii_norm_z,IntegralImage <float> ii_valid_normal, int size,int width,int height)
{
	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}

	for(int i = size; i < width-size; i++)
	{
		for(int j = size; j < height-size; j++)
		{
			int ipsize = i+size;
			int jpsize = j+size;
			int insize = i-size;
			int jnsize = j-size;
			
			float px_norm_x		= ii_norm_x.sum(			ipsize,	jpsize,	i,		jnsize);
			float px_norm_y 	= ii_norm_y.sum(			ipsize,	jpsize,	i,		jnsize);
			float px_norm_z 	= ii_norm_z.sum(			ipsize,	jpsize,	i,		jnsize);
			float divpx 		= ii_valid_normal.sum(		ipsize,	jpsize,	i,		jnsize);
			
			if(divpx > 0)		{
				float mul_divpx		= 1/divpx;
				px_norm_x *= mul_divpx;
				px_norm_y *= mul_divpx;
				px_norm_z *= mul_divpx;
			}
			
			float nx_norm_x 	= ii_norm_x.sum(			i,		jpsize,	insize,	jnsize);
			float nx_norm_y 	= ii_norm_y.sum(			i,		jpsize,	insize,	jnsize);
			float nx_norm_z 	= ii_norm_z.sum(			i,		jpsize,	insize,	jnsize);
			float divnx 		= ii_valid_normal.sum(		i,		jpsize,	insize,	jnsize);
			
			if(divnx > 0)		{
				float mul_divnx		= 1/divnx;
				nx_norm_x *= mul_divnx;
				nx_norm_y *= mul_divnx;
				nx_norm_z *= mul_divnx;
			}
			
			float dx_norm_x		= fabs(px_norm_x-nx_norm_x);
			float dx_norm_y		= fabs(px_norm_y-nx_norm_y);
			float dx_norm_z		= fabs(px_norm_z-nx_norm_z);
			
			float py_norm_x 	= ii_norm_x.sum(			ipsize,	jpsize,	insize,	j);
			float py_norm_y 	= ii_norm_y.sum(			ipsize,	jpsize,	insize,	j);
			float py_norm_z 	= ii_norm_z.sum(			ipsize,	jpsize,	insize,	j);
			float divpy 		= ii_valid_normal.sum(		ipsize,	jpsize,	insize,	j);
			
			if(divpy > 0)		{
				float mul_divpy		= 1/divpy;
				py_norm_x *= mul_divpy;
				py_norm_y *= mul_divpy;
				py_norm_z *= mul_divpy;
			}
			
			float ny_norm_x		= ii_norm_x.sum(			ipsize,	j,		insize,	jnsize);
			float ny_norm_y		= ii_norm_y.sum(			ipsize,	j,		insize,	jnsize);
			float ny_norm_z		= ii_norm_z.sum(			ipsize,	j,		insize,	jnsize);
			float divny 		= ii_valid_normal.sum(		ipsize,	j,		insize,	jnsize);
			if(divny > 0)		{
				float mul_divny		= 1/divny;
				ny_norm_x *= mul_divny;
				ny_norm_y *= mul_divny;
				ny_norm_z *= mul_divny;
			}
			
			float dy_norm_x		= fabs(py_norm_x-ny_norm_x);
			float dy_norm_y		= fabs(py_norm_y-ny_norm_y);
			float dy_norm_z		= fabs(py_norm_z-ny_norm_z);
			float div 			= ii_valid_normal.sum(		ipsize,	jpsize,	insize,	jnsize);
			//float total 		= ii_normal.sum(		i+size,	j+size,	i-size,	j-size);
			if(divpx < size || divnx < size || divpy < size || divny < size){
				edge[i][j] 		= -1;
			}else{
				edge[i][j] 		= 130.0f*sqrt(dx_norm_x*dx_norm_x+dx_norm_y*dx_norm_y+dx_norm_z*dx_norm_z + dy_norm_x*dy_norm_x+dy_norm_y*dy_norm_y+dy_norm_z*dy_norm_z)/div;
			}
		}
	}
	return edge;
}

float ** estimateRGBEdge(IntegralImage <int> ii_r, IntegralImage <int> ii_g, IntegralImage <int> ii_b, int size,int width,int height)
{
	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}
	
	for(int i = size; i < width-size; i++)
	{
		for(int j = size; j < height-size; j++)
		{
			int ipsize = i+size;
			int jpsize = j+size;
			int insize = i-size;
			int jnsize = j-size;
			
			float px_norm_x		= ii_r.sum(			ipsize,	jpsize,	i,		jnsize);
			float px_norm_y 	= ii_g.sum(			ipsize,	jpsize,	i,		jnsize);
			float px_norm_z 	= ii_b.sum(			ipsize,	jpsize,	i,		jnsize);

			
			float nx_norm_x 	= ii_r.sum(			i,		jpsize,	insize,	jnsize);
			float nx_norm_y 	= ii_g.sum(			i,		jpsize,	insize,	jnsize);
			float nx_norm_z 	= ii_b.sum(			i,		jpsize,	insize,	jnsize);

			
			float dx_norm_x		= fabs(px_norm_x-nx_norm_x);
			float dx_norm_y		= fabs(px_norm_y-nx_norm_y);
			float dx_norm_z		= fabs(px_norm_z-nx_norm_z);
			
			float py_norm_x 	= ii_r.sum(			ipsize,	jpsize,	insize,	j);
			float py_norm_y 	= ii_g.sum(			ipsize,	jpsize,	insize,	j);
			float py_norm_z 	= ii_b.sum(			ipsize,	jpsize,	insize,	j);
			
			float ny_norm_x		= ii_r.sum(			ipsize,	j,		insize,	jnsize);
			float ny_norm_y		= ii_g.sum(			ipsize,	j,		insize,	jnsize);
			float ny_norm_z		= ii_b.sum(			ipsize,	j,		insize,	jnsize);

			float dy_norm_x		= fabs(py_norm_x-ny_norm_x);
			float dy_norm_y		= fabs(py_norm_y-ny_norm_y);
			float dy_norm_z		= fabs(py_norm_z-ny_norm_z);
			
			edge[i][j] 			= 0.0001f*sqrt(dx_norm_x*dx_norm_x+dx_norm_y*dx_norm_y+dx_norm_z*dx_norm_z + dy_norm_x*dy_norm_x+dy_norm_y*dy_norm_y+dy_norm_z*dy_norm_z);
		}
	}
	return edge;
}


void segment(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr input_cloud)
{
	int width 	= input_cloud->width;
	int height 	= input_cloud->height;
	ROS_INFO("+++++++++++++++++++++++++++++++++++segmentation started+++++++++++++++++++++++++++++++++++");
	struct timeval start, end;
	gettimeofday(&start, NULL);
	float ** norm_x 				= new float *[width];
	float ** norm_y 				= new float *[width];
	float ** norm_z 				= new float *[width];
	float ** x						= new float *[width];
	float ** y						= new float *[width];
	float ** z	 					= new float *[width];
	int ** r 						= new int *[width];
	int ** g 						= new int *[width];
	int ** b 						= new int *[width];
	float ** valid_depth 			= new float *[width];
	float ** valid_normal 			= new float *[width];
	pcl::PointXYZRGBNormal *** pointmat 	= new pcl::PointXYZRGBNormal **[width];
	for(int i = 0; i < width; i++)
	{
		norm_x[i]			= new float [height];
		norm_y[i]			= new float [height];
		norm_z[i]			= new float [height];
		
		x[i]				= new float [height];
		y[i]				= new float [height];
		z[i]				= new float [height];
		
		r[i]				= new int [height];
		g[i]				= new int [height];
		b[i]				= new int [height];
		valid_depth[i]		= new float [height];
		valid_normal[i]		= new float [height];
		pointmat[i]			= new pcl::PointXYZRGBNormal*[height];
	}
	gettimeofday(&end, NULL);
	ROS_INFO("setup execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);

	gettimeofday(&start, NULL);
	int index = 0;
	float max_depth = -1;
	for(int j = 0; j < height; j++)
	{
		for(int i = 0; i < width; i++)
		{
			pcl::PointXYZRGBNormal point = input_cloud->points[index];
			pointmat[i][j] 			= &point;
			//depth[i][j]			=point.z;
			//if(point.z > max_depth){max_depth = point.z;}
			//depth[i][j]			=1;
			
			r[i][j]				=point.r;
			g[i][j]				=point.g;
			b[i][j]				=point.b;
			if(point.z > 0 && !std::isnan(point.z))	{
				valid_depth[i][j]	= 1;
				x[i][j]				= point.x;
				y[i][j]				= point.y;
				z[i][j]				= point.z;
				if(point.z > max_depth){max_depth = point.z;}
			}
			else{
				valid_depth[i][j]	= 0;
				z[i][j]			= 0;
			}
			if(!std::isnan(point.normal_x))	{
				valid_normal[i][j]	= 1;
				norm_x[i][j]		=point.normal_x;
				norm_y[i][j]		=point.normal_y;
				norm_z[i][j]		=point.normal_z;
			}
			else{
				valid_normal[i][j]	= 0;
				norm_x[i][j]		= 0;
				norm_y[i][j]		= 0;
				norm_z[i][j]		= 0;
			}
			index++;
		}
	}
	gettimeofday(&end, NULL);
	ROS_INFO("fill execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	gettimeofday(&start, NULL);
	
	IntegralImage <int> ii_r;
	ii_r.setup(r,width,height);
	IntegralImage <int> ii_g;
	ii_g.setup(g,width,height);
	IntegralImage <int> ii_b;
	ii_b.setup(b,width,height);
	
	IntegralImage <float> ii_norm_x;
	ii_norm_x.setup(norm_x,width,height);
	IntegralImage <float> ii_norm_y;
	ii_norm_y.setup(norm_y,width,height);
	IntegralImage <float> ii_norm_z;
	ii_norm_z.setup(norm_z,width,height);
	
	IntegralImage <float> ii_valid_normal;
	ii_valid_normal.setup(valid_normal,width,height);
	IntegralImage <float> ii_valid_depth;
	ii_valid_depth.setup(valid_depth,width,height);
	
	IntegralImage <float> ii_depth;
	ii_depth.setup(z,width,height);
	
	gettimeofday(&end, NULL);
	ROS_INFO("IntegralImage setup execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	gettimeofday(&start, NULL);	
		
	float ** depth_edge 		= estimateDepthEdge(	ii_depth,	ii_valid_depth,										4,	width,	height);
	float ** normal_edge 		= estimateNormalEdge(	ii_norm_x, 	ii_norm_y, 		ii_norm_z,		ii_valid_normal, 	4, 	width, 	height);
	float ** rgb_edge 			= estimateRGBEdge(		ii_r, 		ii_g, 			ii_b, 								3, 	width, 	height);
	
	float depth_edge_max 	= -1;
	float normal_edge_max 	= -1;
	float rgb_edge_max 		= -1;
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			if(depth_edge_max 	< depth_edge[i][j])	{depth_edge_max 	= depth_edge[i][j];	}
			if(normal_edge_max 	< normal_edge[i][j]){normal_edge_max 	= normal_edge[i][j];}
			if(rgb_edge_max 	< rgb_edge[i][j])	{rgb_edge_max 		= rgb_edge[i][j];	}	
		}
	}
	
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			depth_edge[i][j]	/=depth_edge_max;
			normal_edge[i][j]	/=normal_edge_max;
			rgb_edge[i][j]		/=rgb_edge_max;	
		}
	}
	
	bool ** total_edge = new bool *[width];
	for(int i = 0; i < width; i++)
	{
		total_edge[i] = new bool[height];
		for(int j = 0; j < height; j++)
		{
			total_edge[i][j] = (rgb_edge[i][j]>0.05 || normal_edge[i][j]>0.05 || depth_edge[i][j]>0.05);	
		}
	}

	gettimeofday(&end, NULL);
	ROS_INFO("Edge extraction execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	gettimeofday(&start, NULL);

	int ** segmentation = new int *[width];
	for(int i = 0; i < width; i++)
	{
		segmentation[i] = new int[height];
		for(int j = 0; j < height; j++)
		{
			segmentation[i][j]= -1;	
		}
	}	
	
	int segments_found = 0;
	std::vector< std::vector<int> * > * segments_w_data = new std::vector< std::vector<int> * >();
	std::vector< std::vector<int> * > * segments_h_data = new std::vector< std::vector<int> * >();
	
	for(int i = 10; i < width-10; i++)
	{

		for(int j = 10; j < height-10; j++)
		{
			if(segmentation[i][j] == -1 && !total_edge[i][j])
			{
				std::vector<int> * w_points = new std::vector<int>();
				std::vector<int> * h_points	= new std::vector<int>();
				w_points->push_back(i);
				h_points->push_back(j);
				int k = 0;
				int nr_valid = 0;
				while(k < w_points->size())
				{
					int w = w_points->at(k);
					//w_points.pop_back();
					int h = h_points->at(k);
					//h_points.pop_back();
					k++;
					if(w-1 >= 10 && segmentation[w-1][h] == -1 && !total_edge[w-1][h]){
						segmentation[w-1][h] = segments_found;
						w_points->push_back(w-1);
						h_points->push_back(h);
						if(z[w-1][h] > 0){nr_valid++;}
					}
					
					if(w+1 < width-10 && segmentation[w+1][h] == -1 && !total_edge[w+1][h]){
						segmentation[w+1][h] = segments_found;
						w_points->push_back(w+1);
						h_points->push_back(h);
						if(z[w+1][h] > 0){nr_valid++;}
					}
					
					if(h+1 < height-10 && segmentation[w][h+1] == -1 && !total_edge[w][h+1]){
						segmentation[w][h+1] = segments_found;
						w_points->push_back(w);
						h_points->push_back(h+1);
						if(z[w][h+1] > 0){nr_valid++;}
					}

					if(h-1 >= 10 && segmentation[w][h-1] == -1 && !total_edge[w][h-1]){
						segmentation[w][h-1] = segments_found;
						w_points->push_back(w);
						h_points->push_back(h-1);
						if(z[w][h-1] > 0){nr_valid++;}
					}
				}
				if(nr_valid > 2000){
					segments_w_data->push_back(w_points);
					segments_h_data->push_back(h_points);
				}else{
					delete w_points;
					delete h_points;
				}
				segments_found++;
			}	
		}
	}
	
	gettimeofday(&end, NULL);
	ROS_INFO("Connected Component execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	gettimeofday(&start, NULL);
		
	IplImage * normal_img 		= cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 3);
	IplImage * rgb_img 			= cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
	IplImage * depth_edge_img 	= cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 3);
	IplImage * normal_edge_img 	= cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 3);
	IplImage * rgb_edge_img 	= cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 3);
	IplImage * mul_edge_img 	= cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 3);
	char * rgb_data 			= rgb_img->imageData;
	float * normal_data 		= (float *)(normal_img->imageData);
	float * depth_edge_data 	= (float *)(depth_edge_img->imageData);
	float * normal_edge_data 	= (float *)(normal_edge_img->imageData);
	float * rgb_edge_data	 	= (float *)(rgb_edge_img->imageData);
	float * mul_edge_data	 	= (float *)(mul_edge_img->imageData);
	
	
	float * random_cols_r 		= new float[segments_found];
	float * random_cols_g 		= new float[segments_found];
	float * random_cols_b 		= new float[segments_found];
	for(int i = 0; i < segments_found; i++)
	{
		random_cols_r[i]		=((float)(rand()%10000))/10000.0f;
		random_cols_g[i]		=((float)(rand()%10000))/10000.0f;
		random_cols_b[i] 		=((float)(rand()%10000))/10000.0f;
	}
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			int ind = 3 * (j * width + i);
			rgb_data[ind+0] = b[i][j];
			rgb_data[ind+1] = g[i][j];
			rgb_data[ind+2] = r[i][j];
			normal_data[ind+0] = 0.5f*(norm_x[i][j]+1);
			normal_data[ind+1] = 0.5f*(norm_y[i][j]+1);
			normal_data[ind+2] = 0.5f*(norm_z[i][j]+1);
			
			if(total_edge[i][j])
			{
				mul_edge_data[ind+0] = 1;
				mul_edge_data[ind+1] = 1;
				mul_edge_data[ind+2] = 1;
			}else if(segmentation[i][j] != -1){
				mul_edge_data[ind+0] = random_cols_b[segmentation[i][j]];
				mul_edge_data[ind+1] = random_cols_g[segmentation[i][j]];
				mul_edge_data[ind+2] = random_cols_r[segmentation[i][j]];
			}
			
			if(depth_edge[i][j] == -1)
			{
				depth_edge_data[ind+0] = 0;
				depth_edge_data[ind+1] = 1;
				depth_edge_data[ind+2] = 0;
			
			}
			else
			{
				depth_edge_data[ind+0] = depth_edge[i][j];
				depth_edge_data[ind+1] = depth_edge[i][j];
				depth_edge_data[ind+2] = depth_edge[i][j];
				
				//mul_edge_data[ind+0] += depth_edge[i][j]*rgb_edge[i][j];
				//mul_edge_data[ind+1] += depth_edge[i][j]*rgb_edge[i][j];
				//mul_edge_data[ind+2] += depth_edge[i][j]*rgb_edge[i][j];
			}
			
			if(normal_edge[i][j] == -1)
			{
				normal_edge_data[ind+0] = 0;
				normal_edge_data[ind+1] = 1;
				normal_edge_data[ind+2] = 0;
				
				//mul_edge_data[ind+0] = 1;
				//mul_edge_data[ind+1] = 1;
				//mul_edge_data[ind+2] = 1;
			}
			else
			{
				normal_edge_data[ind+0] = normal_edge[i][j];
				normal_edge_data[ind+1] = normal_edge[i][j];
				normal_edge_data[ind+2] = normal_edge[i][j];
				
				//mul_edge_data[ind+0] += normal_edge[i][j];
				//mul_edge_data[ind+1] += normal_edge[i][j];
				//mul_edge_data[ind+2] += normal_edge[i][j];
			}
			
			if(rgb_edge[i][j] == -1)
			{
				rgb_edge_data[ind+0] = 0;
				rgb_edge_data[ind+1] = 1;
				rgb_edge_data[ind+2] = 0;
				
				//mul_edge_data[ind+0] = 0;
				//mul_edge_data[ind+1] = 1;
				//mul_edge_data[ind+2] = 0;
			}
			else
			{
				rgb_edge_data[ind+0] = rgb_edge[i][j]>0.1f;
				rgb_edge_data[ind+1] = rgb_edge[i][j]>0.1f;
				rgb_edge_data[ind+2] = rgb_edge[i][j]>0.1f;

				//mul_edge_data[ind+0] *= 10.0f*rgb_edge[i][j] >= 1;
				//mul_edge_data[ind+1] *= 10.0f*rgb_edge[i][j] >= 1;
				//mul_edge_data[ind+2] *= 10.0f*rgb_edge[i][j] >= 1;
			}
		}
	}

	cvNamedWindow("InputCloud_rgb", CV_WINDOW_AUTOSIZE );
	cvShowImage("InputCloud_rgb", rgb_img);
	
	//cvNamedWindow("InputCloud_normal", CV_WINDOW_AUTOSIZE );
	//cvShowImage("InputCloud_normal", normal_img);
	
	//cvNamedWindow("InputCloud_depth_edge", CV_WINDOW_AUTOSIZE );
	//cvShowImage("InputCloud_depth_edge", depth_edge_img);
	
	//cvNamedWindow("InputCloud_normal_edge", CV_WINDOW_AUTOSIZE );
	//cvShowImage("InputCloud_normal_edge", normal_edge_img);
	
	//cvNamedWindow("InputCloud_rgb_edge", CV_WINDOW_AUTOSIZE );
	//cvShowImage("InputCloud_rgb_edge", rgb_edge_img);
	
	cvNamedWindow("InputCloud_mul_edge", CV_WINDOW_AUTOSIZE );
	cvShowImage("InputCloud_mul_edge", mul_edge_img);
	
	cvWaitKey(0);

	gettimeofday(&end, NULL);
	ROS_INFO("vizualization time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	gettimeofday(&start, NULL);
	
	std::vector<Plane * > * planes = new std::vector<Plane * >();
	std::vector<int> binsizes;
	binsizes.push_back(4);
	binsizes.push_back(4);
	binsizes.push_back(4);
	
	std::vector<float> max_val;
	max_val.push_back(255);
	max_val.push_back(255);
	max_val.push_back(255);
	
	std::vector<float> min_val;
	min_val.push_back(0);
	min_val.push_back(0);
	min_val.push_back(0);
	
	for(unsigned int i = 0; i < segments_w_data->size(); i++)
	{

		std::vector<int> * seg_w = segments_w_data->at(i);
		std::vector<int> * seg_h = segments_h_data->at(i);
		int seg_ind = segmentation[seg_w->at(0)][seg_h->at(0)];
		ROS_INFO("new segment: %i",seg_ind);
		std::vector<Point * > * seg = new std::vector<Point * >();
		std::vector<FeatureDescriptor * > * rgb_descs = new std::vector<FeatureDescriptor * >();
		for(unsigned int j = 0; j < seg_w->size(); j++)
		{
			int w = seg_w->at(j);
			int h = seg_h->at(j);
			Point * point = new Point(x[w][h],y[w][h],z[w][h],w,h);
			rgb_descs->push_back(new RGBFeatureDescriptor(r[w][h],g[w][h],b[w][h]));
			if(point->z > 0){seg->push_back(point);}
		}
		Histogram * colhist = new Histogram(rgb_descs, binsizes, max_val, min_val);
		Plane * plane = new Plane(seg);
		planes->push_back(plane);
		plane->color_histogram = colhist;
		for(unsigned int j = 0; j < seg_w->size(); j++){delete rgb_descs->at(j);}
		for(unsigned int j = 0; j < seg->size(); j++){delete seg->at(j);}
		delete seg;
		delete rgb_descs;
	}
	
	cvNamedWindow("InputCloud_rgb", CV_WINDOW_AUTOSIZE );
	cvShowImage("InputCloud_rgb", rgb_img);
	cvWaitKey(0);

	for(int i = 0; i < planes->size(); i++)
	{
		Plane * pa = planes->at(i);
		std::vector<int> * seg_wi = segments_w_data->at(i);
		std::vector<int> * seg_hi = segments_h_data->at(i);
		
		
		for(int j = i+1; j < planes->size(); j++)
		{
			ROS_INFO("planes->size() = %i",planes->size());
			IplImage * rgb_img_clone = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
			cvCopy( rgb_img, rgb_img_clone, NULL );
			std::vector<int> * seg_wj = segments_w_data->at(j);
			std::vector<int> * seg_hj = segments_h_data->at(j);
			
			for(float k = 0; k < seg_wi->size(); k++)
			{
				int w = seg_wi->at(k);
				int h = seg_hi->at(k);
				//ROS_INFO("631: w = %i, h = %i",w,h);
				rgb_img_clone->imageData[3*(h*width+w)+0] = 255;
				rgb_img_clone->imageData[3*(h*width+w)+1] = 0;
				rgb_img_clone->imageData[3*(h*width+w)+2] = 255;
			}
			
			for(float k = 0; k < seg_wj->size(); k++)
			{
				int w = seg_wj->at(k);
				int h = seg_hj->at(k);
				//ROS_INFO("641: w = %i, h = %i",w,h);
				rgb_img_clone->imageData[3*(h*width+w)+0] = 0;
				rgb_img_clone->imageData[3*(h*width+w)+1] = 255;
				rgb_img_clone->imageData[3*(h*width+w)+2] = 0;
			}
			
			Plane * pb = planes->at(j);
			float coldist =   pa->color_histogram->distance(pb->color_histogram);
			float angle = fabs(pa->normal_x * pb->normal_x + pa->normal_y * pb->normal_y + pa->normal_z * pb->normal_z);
			
			Point * pa_mean = new Point(pa->point_x,pa->point_y,pa->point_z,0,0);
			Point * pb_mean = new Point(pb->point_x,pb->point_y,pb->point_z,0,0);
			float dist1 = fabs(pa->distance(pb_mean));
			float dist2 = fabs(pb->distance(pa_mean));
			delete pa_mean;
			delete pb_mean;	
			ROS_INFO("angle: %f, dist1: %f dist2: %f coldist: %f",angle,dist1,dist2,coldist);				
			if(angle > 0.990f && dist1 < 0.05&& dist2 < 0.05 && coldist < 0.05)
			{
				
				//pa->merge(pb);
				//planes->at(j) = planes->at(planes->size() - 1);
				//planes->at(planes->size() - 1) = pb;
				//planes->pop_back();
				delete pa;
				delete pb;

				while(seg_wj->size() > 0)
				{
					seg_wi->push_back(seg_wj->back());
					seg_hi->push_back(seg_hj->back());
					seg_wj->pop_back();
					seg_hj->pop_back();
				}

				std::vector<Point * > * seg = new std::vector<Point * >();
				std::vector<FeatureDescriptor * > * rgb_descs = new std::vector<FeatureDescriptor * >();
								
				for(float k = 0; k < seg_wi->size(); k++)
				{
					int w = seg_wi->at(k);
					int h = seg_hi->at(k);
					
					Point * point = new Point(x[w][h],y[w][h],z[w][h],w,h);
					rgb_descs->push_back(new RGBFeatureDescriptor(r[w][h],g[w][h],b[w][h]));
					if(point->z > 0){seg->push_back(point);}

				}
				
				pa = new Plane(seg);
				pa->color_histogram = new Histogram(rgb_descs, binsizes, max_val, min_val);
				
				for(unsigned int k = 0; k < rgb_descs->size(); 	k++)	{delete rgb_descs->at(k);}
				for(unsigned int k = 0; k < seg->size(); 		k++)	{delete seg->at(k);}
				delete seg;
				delete rgb_descs;
				
				planes->at(i) = pa;
				planes->at(j) = planes->at(planes->size() - 1);
				planes->pop_back();
				
				segments_w_data->at(j) = segments_w_data->at(planes->size()-1);
				segments_h_data->at(j) = segments_h_data->at(planes->size()-1);
				segments_w_data->at(planes->size()-1) = seg_wj;
				segments_h_data->at(planes->size()-1) = seg_hj;
				j--;
			}
			cvNamedWindow("InputCloud_rgb", CV_WINDOW_AUTOSIZE );
			cvShowImage("InputCloud_rgb", rgb_img_clone);
			cvWaitKey(0);
			cvReleaseImage( &rgb_img_clone );
		}
	}
	
	for(int i = 0; i < planes->size(); i++)
	{
		Plane * pa = planes->at(i);
		for(int j = i+1; j < planes->size(); j++)
		{
			IplImage * rgb_img_clone = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
			cvCopy( rgb_img, rgb_img_clone, NULL );
			
			Plane * pb = planes->at(j);

			Line * line = new Line(pa,pb);
			
			std::vector<int> * seg_w = segments_w_data->at(i);
			std::vector<int> * seg_h = segments_h_data->at(i);
			
			for(float k = 0; k < seg_w->size(); k++)
			{
				int w = seg_w->at(k);
				int h = seg_h->at(k);
				Point * point = new Point(x[w][h],y[w][h],z[w][h],w,h);
				if(point->z > 0){
					if(line->distance(point) < 0.03f){
						rgb_img_clone->imageData[3*(h*width+w)+0] = 0;
						rgb_img_clone->imageData[3*(h*width+w)+1] = 0;
						rgb_img_clone->imageData[3*(h*width+w)+2] = 255;
					}else{
						rgb_img_clone->imageData[3*(h*width+w)+0] = 255;
						rgb_img_clone->imageData[3*(h*width+w)+1] = 0;
						rgb_img_clone->imageData[3*(h*width+w)+2] = 255;
					}
				}
				delete point;
			}
			
			seg_w = segments_w_data->at(j);
			seg_h = segments_h_data->at(j);
			
			for(float k = 0; k < seg_w->size(); k++)
			{
				int w = seg_w->at(k);
				int h = seg_h->at(k);
				Point * point = new Point(x[w][h],y[w][h],z[w][h],w,h);
				if(point->z > 0){
					if(line->distance(point) < 0.05f){
						rgb_img_clone->imageData[3*(h*width+w)+0] = 0;
						rgb_img_clone->imageData[3*(h*width+w)+1] = 0;
						rgb_img_clone->imageData[3*(h*width+w)+2] = 255;
					}else{
						rgb_img_clone->imageData[3*(h*width+w)+0] = 0;
						rgb_img_clone->imageData[3*(h*width+w)+1] = 255;
						rgb_img_clone->imageData[3*(h*width+w)+2] = 0;
					}
				}
				delete point;
			}
					
           	cvNamedWindow("InputCloud_rgb", CV_WINDOW_AUTOSIZE );
			cvShowImage("InputCloud_rgb", rgb_img_clone);
			cvWaitKey(0);
			cvReleaseImage( &rgb_img_clone );
		}
	}
	
	//cvCopy( src, dst, NULL );
	
	gettimeofday(&end, NULL);
	ROS_INFO("Planar fit time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	gettimeofday(&start, NULL);
	
	ii_r.clear();
	ii_g.clear();
	ii_b.clear();
	ii_norm_x.clear();
	ii_norm_y.clear();
	ii_norm_z.clear();
	ii_valid_normal.clear();
	ii_valid_depth.clear();
	ii_depth.clear();
	
	for(int i = 0; i < width; i++)
	{
		delete norm_x[i];
		delete norm_y[i];
		delete norm_z[i];
		delete x[i];
		delete y[i];
		delete z[i];
		delete r[i];
		delete g[i];
		delete b[i];
		delete valid_depth[i];
		delete valid_normal[i];
	}
	delete norm_x;
	delete norm_y;
	delete norm_z;
	
	delete x;
	delete y;
	delete z;
	
	delete r;
	delete g;
	delete b;
	delete valid_depth;
	delete valid_normal;
	cvReleaseImage( &rgb_img );
	cvReleaseImage( &normal_img );
	cvReleaseImage( &depth_edge_img );
	cvReleaseImage( &normal_edge_img );
	
	gettimeofday(&end, NULL);
	ROS_INFO("Cleanup execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	ROS_INFO("segmentation ended");
}
